#include "PID.h"

extern uint8_t flightMode;
extern float input[6];//输入数据，横滚、俯仰、油门、偏航、直接力、舵面来流指向，归一法
extern float feedback[6];//横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
extern float ctrl[6];//控制数据，横滚、俯仰、油门、偏航、直接力、舵面来流指向，归一法
extern float trueAOA;//返回真实AOA数据，单位为deg

void PID()
{
  float x;//内外环变量传递参数

  switch (flightMode)
  {//根据飞行模式切换运算
    case 1://手动模式
      ctrl[0] = PID_RR(input[0],feedback[0],rrP,rrI,rrD,1);//横滚以输入控制
      ctrl[1] = PID_AOA(input[1],feedback[3],paP,paI,paD,1);//俯仰以输入控制
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,1);//偏航以输入控制
      break;

    case 2://攻角模式
      //横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
      ctrl[0] = PID_RR(input[0],feedback[0],rrP,rrI,rrD,0);//横滚以输入控制
      ctrl[1] = PID_AOA(input[1],feedback[3],paP,paI,paD,0);//俯仰以攻角控制
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,0);//偏航以输入控制
      break;

    case 3://攻角速率双闭环模式
      //横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
      ctrl[0] = PID_RR(input[0],feedback[0],rrP,rrI,rrD,0);//横滚以横滚速率控制
      x = PID_PR(input[1],feedback[1],prP,prI,prD,0);//攻角指令由俯仰速率控制器得出
      ctrl[1] = PID_AOA(x,feedback[3],paP,paI,paD,0);//俯仰以攻角控制
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,0);//偏航以偏航速率控制
      break;

    case 4://半自稳模式
      //横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
      ctrl[0] = PID_RR(input[0],feedback[0],rrP,rrI,rrD,0);//横滚以横滚速率控制
      ctrl[1] = PID_PR(input[1],feedback[1],prP2,prI2,prD2,0);//俯仰以俯仰速率控制
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,0);//偏航以偏航速率控制
      break;

    default://失控或自稳模式
      break;
  }

  ctrl[2] = input[2];//电机直接控制
  ctrl[4] = input[4];//直接力控制直接以输入控制
  ctrl[5] = input[5] * constrain(mapF(trueAOA,-45,45,-1,1),-1,1);//舵面来流指向直接以输入乘攻角
}

float PID_AOA(float in,float fb,float p,float i,float d,bool rOUT)
{//攻角控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}

float PID_PR(float in,float fb,float p,float i,float d,bool rOUT)
{//俯仰速率控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}

float PID_RR(float in,float fb,float p,float i,float d,bool rOUT)
{//横滚速率控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}

float PID_YR(float in,float fb,float p,float i,float d,bool rOUT)
{//偏航速率控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}
